/*
 * @Description: voxel filter 模块实现
 * @Author: Zhijian Qiao
 * @Date: 2020-02-09 19:53:20
 */
#include "models/cloud_filter/voxel_filter.hpp"
#include "glog/logging.h"
#include <pcl/common/common.h>
#include <pcl/filters/crop_box.h>

namespace avp_slam {

    VoxelFilter::VoxelFilter(const YAML::Node &node) {
        float leaf_size_x = node["leaf_size"][0].as<float>();
        float leaf_size_y = node["leaf_size"][1].as<float>();
        float leaf_size_z = node["leaf_size"][2].as<float>();

        SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z);
    }

    VoxelFilter::VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z) {
        SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z);
    }

    bool VoxelFilter::SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z) {
        voxel_filter_.setLeafSize(leaf_size_x, leaf_size_y, leaf_size_z);

        LOG(INFO) << "Voxel Filter 的参数为：" << std::endl
                  << leaf_size_x << ", "
                  << leaf_size_y << ", "
                  << leaf_size_z
                  << std::endl << std::endl;

        return true;
    }

    bool VoxelFilter::Filter(const CloudData::CLOUD_PTR &input_cloud_ptr, CloudData::CLOUD_PTR &filtered_cloud_ptr) {

        voxel_filter_.setInputCloud(input_cloud_ptr);
        voxel_filter_.filter(*filtered_cloud_ptr);

        return true;
    }

    bool
    VoxelFilter::RobustFilter(const CloudData::CLOUD_PTR &input_cloud_ptr, CloudData::CLOUD_PTR &filtered_cloud_ptr) {

        CloudData::POINT min;    //xyz的最小值
        CloudData::POINT max;    //xyz的最大值
        pcl::getMinMax3D(*input_cloud_ptr, min, max);    //获取所有点中的坐标最值

        const double cube_length = 50;
        int nx = (max.x - min.x) / cube_length + 1;
        int ny = (max.y - min.y) / cube_length + 1;
        int nz = (max.z - min.z) / cube_length + 1;

        const pcl::PointCloud<CloudData::POINT>::Ptr cloudTmp(new pcl::PointCloud<CloudData::POINT>());
        pcl::copyPointCloud(*input_cloud_ptr, *cloudTmp);
        filtered_cloud_ptr->clear();

        pcl::CropBox<CloudData::POINT> pcl_box_filter_;
        pcl_box_filter_.setInputCloud(cloudTmp);
        std::vector<pcl::PointCloud<CloudData::POINT>::Ptr> pointAreas;
        pointAreas.resize(nx * ny * nz);
        for (int x = 0; x < nx; x++)
            for (int y = 0; y < ny; y++)
                for (int z = 0; z < nz; z++) {
                    uint32_t index = x * ny * nz + y * nz + z;
                    pointAreas[index].reset(new pcl::PointCloud<CloudData::POINT>());
                    pcl_box_filter_.setMin(Eigen::Vector4f(min.x + x * cube_length, min.y + y * cube_length,
                                                           min.z + z * cube_length, 1.0));
                    pcl_box_filter_.setMax(Eigen::Vector4f(min.x + (x + 1) * cube_length, min.y + (y + 1) * cube_length,
                                                           min.z + (z + 1) * cube_length, 1.0));
                    pcl_box_filter_.filter(*pointAreas[index]);
                    voxel_filter_.setInputCloud(pointAreas[index]);
                    voxel_filter_.filter(*pointAreas[index]);
                    *filtered_cloud_ptr += *pointAreas[index];
                }

        return true;
    }
} 